#!/usr/bin/python
# coding=UTF-8
"""
命名规范：
类名：CameraClass
函数名：首字母大写
实例化的对象：小写，下划线，加Ob
ject

logging用法：
现在只使用logging.debug和logging.error
main文件里面使用**********debug msg********** （左右各10个）或者 debug msg in main
modeltest 文件使用######debug msg###### （左右各5个）或者 debug msg in modeltest
所有文件打印变量信息使用 filename->argument{}.format(xx)
"""

# from detectors import TaskDetector
from camera import front_cam_Object, right_cam_Object, left_cam_Object
from cruise import cruise_Object
from modeltest import *
from serialport import eb_serial_Object
from lock import *
import math
import logging
import config  # 为改为多进程做准备

logging.basicConfig(level=logging.DEBUG, format='%(levelname)s: %(message)s')

"""
camera cruise detect 开启进程
"""
front_cam_Object.start()
cruise_Object.start()  # 没有利用到类内变量
detect_Object.start()
logging.debug('finish start front_cam cruise detect in the beginning in main')
"""
全局变量
"""
serial_read_result = None
serial_read_flag = False
fortress_order = 0  # 第几次检测到城池
taskorder=0 #任务次序记录
flag_detected = True  # 用于防止多次进入发送计步的 if 里面
flag_slow = True  # 用于防止多次进入发送在地标前减速的if里面
"""
main loop
"""
if __name__ == "__main__":
    while 1:
        if y_detect_model.value > 45 and 95 < x_detect_model.value < 250 and flag_slow:  # 主要是看x的坐标范围  # TODO:待修改
            flag_slow = False
            eb_serial_Object.write_data([0xAA, 0xDE, 1, 2200 // 10, 0, 0xFF])
            logging.debug('**********send flag_slow in main**********')
        if (not STATUS_detect.value) and RESULT_INDEX_detect.value != 0 and flag_detected:  # 地标检测停止，且地标检测的结果不为背景\
            # distance = (-0.1624)*y_detect_model.value + 35.566 + 19 # y_detect_model.value用作DetectModel和main进程之间的通信
            taskorder = taskorder + 1 #每次识别到一个任务的地标就加一
            distance = (-0.1624)*y_detect_model.value + 35.566 + 10
            logging.debug("main->index->distance={}".format(distance))
            if distance < 0:
                distance = 0
            elif distance > 25:
                distance =25
            cruise_Object.stop()  # 关闭巡航线程
            front_cam_Object.stop()  # 关闭前摄像头线程
            logging.debug('**********stop detect in main**********')
            if RESULT_INDEX_detect.value == 1:  # 目标检测中的宿营地（平移）
                eb_serial_Object.write_data([0xAA, 0xD1, round(distance*10), 0, 0, 0xFF])
                flag_detected = False  # 为了只让 发送计步指令 执行一次
            elif RESULT_INDEX_detect.value == 2:  # 封狼居胥（夹方块）
                eb_serial_Object.write_data([0xAA, 0xD2, round(distance*10), 0, 0, 0xFF])
                flag_detected = False
            elif RESULT_INDEX_detect.value == 3:  # 城池
                flag_detected = False
                fortress_order = fortress_order + 1  # 检测到一次就加1
                if taskorder > 2 and fortress_order!=3:
                    fortress_order = fortress_order + 1
                if (fortress_order % 3) == 2:
                    eb_serial_Object.write_data([0xAA, 0xD3, round(distance*7), 0, 0, 0xFF])
                else:
                    eb_serial_Object.write_data([0xAA, 0xD3, round(distance*10), 0, 0, 0xFF])
            elif RESULT_INDEX_detect.value == 4:  # 士兵（倒小球）
                eb_serial_Object.write_data([0xAA, 0xD4, round(distance*10), 0, 0, 0xFF])
                flag_detected = False
            elif RESULT_INDEX_detect.value == 5:  # 打靶
                eb_serial_Object.write_data([0xAA, 0xD5, round(distance*10), 0, 0, 0xFF])
                flag_detected = False
            else:
                pass
        # end!!! if (not STATUS_detect.value) and RESULT_INDEX_detect.value != 0 and flag_detected:

        serial_read_result = eb_serial_Object.read_data()  # 读串口数据
        if serial_read_result is None:
            logging.debug('wait k66 send cf or pass in main')
            pass
        elif serial_read_result == 'cf':  # count finish 计步完成
            logging.debug('**********receive cf in main**********')
            # 根据任务开启左右摄像头
            if RESULT_INDEX_detect.value == 5:  # 打靶
                right_cam_Object.start()  # 开启右摄像头
                time.sleep(0.3)
                while True:  # TODO:所有这些调整函数加入超时停止功能
                    try:
                        if task_Object.test_task(1, 2):  # 侧面检测模型开始，1 表示传进去右摄像头,2表示不resize图片
                            delta_x = task_Object.x - 157  # TODO：之前152
                            delta_y = 119 - task_Object.y  # TODO：之前139
                            if -4 <= delta_x <= 4 :
                                logging.debug("main->index5->delta_x={}".format(delta_x))
                                logging.debug('**********sending target shoot error**********')
                                eb_serial_Object.write_data([0xAA, 0xA1, (0 + 200) // 2, (0 + 200) // 2, 0, 0xFF])  # 发送偏差为0，即停车
                                time.sleep(0.1)
                                break
                            else:
                                eb_serial_Object.write_data(
                                    [0xAA, 0xA1, (delta_x + 200) // 2, (0 + 200) // 2, 0, 0xFF])  # 调整姿态的指令
                                logging.debug("main->index5->delta_x={}".format(delta_x))
                                # logging.debug("main->index5->delta_y={}".format(delta_y))
                                logging.debug('**********sending target shoot error**********')
                            # if math.pow(delta_x, 2) + math.pow(delta_y, 2) < 100:  # 偏差为允许范围内
                            # break
                            
                    except Exception as e:
                        logging.error("raise a error at RESULT_INDEX_detect.value == 5 in main")
                        logging.error(e)  # 打印错误
                        continue

                
                time.sleep(1)  # 延时1s发送 TODO:
                eb_serial_Object.write_data([0xAA, 0xAC, 5, 0, 0, 0xFF])  # 发送打靶的动作
                while True:  # 等待下位机完成任务
                    logging.debug('wait K66 send finish task flag index5 in main')
                    abc5 = eb_serial_Object.read_data()
                    if abc5 == 'af':
                        logging.debug('**********receive af at index5**********')
                        break

                right_cam_Object.stop()  # 关闭摄像头，开启前面摄像头，开始巡线线程
                time.sleep(0.1)
                front_cam_Object.start()
                time.sleep(0.1)
                cruise_Object.start()
                flag_detected = True
                flag_slow = True
                detect_Object.start()
            # end!!! if RESULT_INDEX_detect.value == 5:    # 打靶

            elif RESULT_INDEX_detect.value == 2:  # 封狼居胥，夹方块
                left_cam_Object.start()  # 开启左摄像头
                time.sleep(0.3)
                while True:
                    try:
                        if task_Object.test_task(2, 2):  # 侧面检测模型开始,数字2是传入左边摄像头
                            delta_x = 75 - task_Object.x  # TODO：之前是60
                            delta_y = task_Object.y - 158  # TODO：需要添加判断是否是靶标吗？
                            eb_serial_Object.write_data(
                                [0xAA, 0xA1, (delta_x + 200) // 2,(0 + 200)//2, 0, 0xFF])  # 调整姿态的指令,夹方块的不需要调整
                            logging.debug("main->index2->delta_x={}".format(delta_x))
                            logging.debug("main->index2->delta_y={}".format(delta_y))
                            logging.debug('**********sending jia fang kuai error**********')
                            # if math.pow(delta_x, 2) + math.pow(delta_y, 2) < 100:  # 偏差为允许范围内
                            # break
                            if -7 <= delta_x <= 7:
                                break
                    except Exception as e:
                        logging.error("raise an error at RESULT_INDEX_detect.value == 2 in main")
                        logging.error(e)  # 打印错误
                        continue

                eb_serial_Object.write_data([0xAA, 0xA1, (0 + 200) // 2, (0 + 200) // 2, 0, 0xFF])  # 发送偏差为0，即停车
                time.sleep(1)  # 延时1ms发送 TODO:完成了！！！
                eb_serial_Object.write_data([0xAA, 0xAC, 2, 0, 0, 0xFF])  # 通知下位机完成任务

                while True:  # 等待下位机完成任务
                    logging.debug('wait K66 send finish task flag index2 in main')
                    if eb_serial_Object.read_data() == 'af':
                        logging.debug('**********receive af at index2**********')
                        eb_serial_Object.write_data([0xAA, 0xDE, 1, 2500 // 10, 0, 0xFF])
                        break

                left_cam_Object.stop()
                time.sleep(0.1)
                front_cam_Object.start()
                time.sleep(0.1)
                cruise_Object.start()
                flag_detected = True
                flag_slow = True
                detect_Object.start()
            # end!!! elif RESULT_INDEX_detect.value == 2:          # 封狼居胥，夹方块

            elif RESULT_INDEX_detect.value == 4:  # （倒小球）

                right_cam_Object.start()  # 开启右摄像头
                time.sleep(0.3)
                while True:
                    try:
                        if task_Object.test_task(1, 2):
                            delta_x = task_Object.x - 100  # TODO：参数待修改
                            delta_y = 209 - task_Object.y  # 7月6改的，之前是反过来的，应该是错的
                            eb_serial_Object.write_data(
                                [0xAA, 0xA1, (delta_x + 200) // 2, (0 + 200) // 2, 0, 0xFF])  # 调整姿态的指令
                            logging.debug("main->index4->delta_x={}".format(delta_x))
                            logging.debug("main->index4->delta_y={}".format(delta_y))
                            logging.debug('**********sending throw ball error**********')
                            if -9 <= delta_x <=9:  # 偏差为允许范围内
                                break
                    except Exception as e:
                        logging.error("raise an error at RESULT_INDEX_detect.value == 4 in main")
                        logging.error(e)
                        continue
                eb_serial_Object.write_data([0xAA, 0xA1, (0 + 200) // 2, (0 + 200) // 2, 0xFF])  # 发送偏差为0，即停车
                time.sleep(1)  # 延时20ms发送 TODO:
                eb_serial_Object.write_data([0xAA, 0xAC, 4, 0, 0, 0xFF])  # 通知下位机完成任务

                while True:  # 等待下位机完成任务
                    logging.debug('wait K66 send finish task flag index4 in main ')
                    if eb_serial_Object.read_data() == 'af':
                        logging.debug('**********receive af at index4**********')
                        break
                
                right_cam_Object.stop()
                time.sleep(0.3)
                front_cam_Object.start()
                time.sleep(0.1)
                cruise_Object.start()
                flag_detected = True
                flag_slow = True
                detect_Object.start()
            # end!!! elif RESULT_INDEX_detect.value == 4:  # 士兵（倒小球）

            elif RESULT_INDEX_detect.value == 1:  # 宿营
                left_cam_Object.start()  # 开启左摄像头
                time.sleep(0.3)
                back_flag = 0
                distance_hotel = 0
                while True:
                    try:
                        if task_Object.test_task(2, 2):
                            if task_Object.x <120:
                                back_flag = int(task_Object.x)
                            else:
                                back_flag = 0
                            #(以下函数下位机8.3v)
                            if task_Object.y < 131:
                                # distance_hotel = -0.3673*task_Object.y+85.4286
                                distance_hotel = -0.3673*task_Object.y+92 
                            else:
                                # distance_hotel = -0.2901*task_Object.y+74.86
                                distance_hotel = -0.29*task_Object.y+82.5
                            break
                    except Exception as e:
                        logging.error("raise an error at RESULT_INDEX_detect.value == 1 in main")
                        logging.error(e)
                        continue
                logging.debug("main->index1->task_Object.x={}".format(task_Object.x))
                logging.debug("main->index1->task_Object.y={}".format(task_Object.y))
                logging.debug("main->index1->distance_hotel={}".format(distance_hotel))
                eb_serial_Object.write_data([0xAA, 0xAC, 1, back_flag, round(distance_hotel), 0xFF])
                while True:  # 等待下位机完成任务
                    logging.debug('wait K66 send finish task flag index1 in main')
                    if eb_serial_Object.read_data() == 'af':
                        logging.debug('**********receive af at index1**********')
                        break

                left_cam_Object.stop()
                time.sleep(0.1)  # 延时一会，等待 left_cam 完全停止
                front_cam_Object.start()
                time.sleep(0.1)
                cruise_Object.start()

                flag_detected = True
                flag_slow = True
                detect_Object.start()
            # end!!! elif RESULT_INDEX_detect.value == 1:  # 宿营

            elif RESULT_INDEX_detect.value == 3:  # 举旗
                logging.debug("main->fortress_order{}".format(fortress_order))
                index = 0
                if (fortress_order % 3) == 2:  # 第二次检测到城池
                    right_cam_Object.start()  # 开启右摄像头
                    overtime_flag = 0
                    while True:  # TODO:所有这些调整函数加入超时停止功能
                        if overtime_flag >= 45: #40是9s，60是12s
                            index = 2
                            break
                        overtime_flag = overtime_flag + 1
                        try:
                            if task_Object.test_task(1, 2):  # 侧面检测模型开始，1 表示传进去右摄像头,2表示不resize图片
                                delta_x = task_Object.x - 260  
                                if -40 < delta_x < 40:
                                    index = task_Object.result_index
                                    logging.debug("main->index3->delta_x={}".format(delta_x))
                                    logging.debug('**********sending raiseflag error**********')
                                    eb_serial_Object.write_data([0xAA, 0xA1, (0 + 200) // 2, (0 + 200) // 2, 0, 0xFF])  # 发送偏差为0，即停车
                                    time.sleep(1)
                                    break
                                else:
                                    eb_serial_Object.write_data(
                                        [0xAA, 0xA1, (delta_x + 200) // 2, (0 + 200) // 2, 0, 0xFF])  # 调整姿态的指令
                                    logging.debug("main->index3->delta_x={}".format(delta_x))
                                    logging.debug('**********sending raiseflag error**********')
                                # if math.pow(delta_x, 2) + math.pow(delta_y, 2) < 100:  # 偏差为允许范围内
                                # break
                                
                        except Exception as e:
                            logging.error("raise a error at RESULT_INDEX_detect.value == 3 in main")
                            logging.error(e)  # 打印错误
                            continue

                    
                    eb_serial_Object.write_data([0xAA, 0xAC, 3, index, 0, 0xFF])  # index 作为信息发送给下位机
                else:
                    left_cam_Object.start()  # 开启左摄像头
                    time.sleep(0.3)
                    overtime_flag = 0
                    while True:  # TODO:所有这些调整函数加入超时停止功能
                        if overtime_flag >= 45: #40是9s，60是12s
                            if (fortress_order % 3) == 1:
                                index = 1
                            else:
                                index = 3
                            break
                        overtime_flag = overtime_flag + 1
                        try:
                            if task_Object.test_task(2, 2):  # 侧面检测模型开始，2 表示传进去左摄像头,2表示不resize图片
                                delta_x = 270 - task_Object.x 
                                if -40 < delta_x < 40: #之前是（-33，40）
                                    index = task_Object.result_index
                                    logging.debug("main->index3->delta_x={}".format(delta_x))
                                    logging.debug('**********sending raiseflag error**********')
                                    eb_serial_Object.write_data([0xAA, 0xA1, (0 + 200) // 2, (0 + 200) // 2, 0, 0xFF])  # 发送偏差为0，即停车
                                    time.sleep(1)
                                    break
                                else:
                                    eb_serial_Object.write_data(
                                        [0xAA, 0xA1, (delta_x + 200) // 2, (0 + 200) // 2, 0, 0xFF])  # 调整姿态的指令
                                    logging.debug("main->index3->delta_x={}".format(delta_x))
                                    logging.debug('**********sending raiseflag error**********')
                        except Exception as e:
                            logging.error("raise a error at RESULT_INDEX_detect.value == 3 in main")
                            logging.error(e)  # 打印错误
                            continue

                    eb_serial_Object.write_data([0xAA, 0xA1, (0 + 200) // 2, (0 + 200) // 2, 0, 0xFF])  # 发送偏差为0，即停车
                    time.sleep(1)
                    eb_serial_Object.write_data([0xAA, 0xAC, 3, index, 0, 0xFF])  # index 作为信息发送给下位机
                while True:  # 等待下位机完成任务
                    logging.debug('wait K66 send finish task flag index3 in main')
                    abc3 = eb_serial_Object.read_data()
                    if abc3 == 'af':
                        logging.debug('**********receive af at index3**********')
                        break

                right_cam_Object.stop()
                left_cam_Object.stop()
                time.sleep(0.1)

                front_cam_Object.start()
                time.sleep(0.1)

                cruise_Object.start()
                flag_detected = True
                flag_slow = True
                detect_Object.start()
            # end!!! elif RESULT_INDEX_detect.value == 3:  # 举旗

            else:  # 表示检测的结果不是想要的
                pass
        # end!!! elif serial_read_result == 'cf':
    # end!!! while 1 in main
# end!!! main
